from pyb import UART, LED
import sensor, image, time
uart = UART(3, 115200)

class Kalman(object):
    def __init__(self, Q, R):
        self.p_last = 150  # 随意取值
        self.p_now = 150  # 随意取值
        self.output = 0
        self.gain = 0
        self.Q = Q
        self.R = R

    def calc(self, current):
        self.p_now = self.p_last + self.Q;
        self.gain = self.p_now / (self.p_now + self.R);
        self.output = self.output + self.gain * (current - self.output);
        self.p_last = (1-self.gain) * self.p_now;
        return self.output

class Mean(object):
    def __init__(self, size=5):
        self.buf = [0*j for j in range(size)]
        self.i = 0

    def calc(self, current):
        self.buf[self.i] = current
        self.i += 1
        if self.i>=len(self.buf):
            self.i = 0
        return sum(self.buf) / len(self.buf)

def approx_to_int(x):
    if x-int(x) >= 0.5:
        return int(x)+1
    else:
        return int(x)

sensor.reset()                      # Reset and initialize the sensor.
sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QVGA)   # Set frame size to QVGA (320x240)
sensor.set_auto_exposure(False, 6576)  #
sensor.set_auto_whitebal(False, (-4.99849, -6.02073, -5.88632))  # 无法精确锁定，实测锁定在(-5.11999, -6.297, -6.1578)
sensor.set_auto_gain(False, 9.54254)  #

sensor.skip_frames(time = 1000)     # Wait for settings take effect.

print("get_exposure_us:", sensor.get_exposure_us())
print("get_rgb_gain_db:", sensor.get_rgb_gain_db())
print("get_gain_db:", sensor.get_gain_db())

sensor.skip_frames(time = 1000)
clock = time.clock()                # Create a clock object to track the FPS.

led = LED(2)
K = 6150  # 测距
kalman_d = Kalman(0.5, 30)
kalman_x = Kalman(3, 5)
mean_d = Mean(10)
mean_x = Mean()

while(True):
    clock.tick()                    # Update the FPS clock.
    led.toggle()
    img = sensor.snapshot()         # Take a picture and return the image.
    # ---------调试---------------
    blobs = img.find_blobs([(10, 90, 46, 80, 2, 51)], pixels_threshold=80, area_threshold=350,
                            merge=True, margin=5, x_stride=12, y_stride=12)
    if blobs:
        if len(blobs) > 1:
            comp = []
            for blob in blobs:
                comp.append(blob.compactness())
            blob = blobs[comp.index(max(comp))]
            del comp
        else:
            blob = blobs[0]
        img.draw_rectangle(blob.rect())  # rect(): (x, y, w, h)
        print(blob.rect())
        c = blob.rect()[3]  # 直径
        d = K/c  # 距离
        if d > 350:
            d = 350
        elif d < 190:
            d = 190
        #output="%03d%03d" % (blob.cx(), d)
        #print(blob.rect())
        x = kalman_x.calc(blob.cx())
        d = kalman_d.calc(d)
        output="%03d%03d" % (approx_to_int(mean_x.calc(x)), approx_to_int(mean_d.calc(d)))
        uart.write("!"+output)
        print(output)
    else:
        output="999999"
        uart.write("!"+output)
        print(output, "no blobs found")

# 3 307 2.5 247 2 199

